 

#include "Elem_Beam_2D_Nodes2.h"
#include "MyTime.h"
#include "BasisFunctionsLagrange.h"
#include "GeomDataLagrange.h"
#include "QuadratureUtil.h"
#include "TimeFunction.h"
#include "util.h"


extern  std::vector<unique_ptr<TimeFunction> > timeFunction;
extern  MyTime           myTime;
extern  bool  debug;

using namespace std;


Elem_Beam_2D_Nodes2::Elem_Beam_2D_Nodes2()
{
  ndof   = 3;
  ndim   = 2;
  npElem = 2;
  nsize  = npElem*ndof;

  if (debug) cout << " constructor Elem_Beam_2D_Nodes2\n\n";
}



Elem_Beam_2D_Nodes2::~Elem_Beam_2D_Nodes2()
{
  if (debug) cout << " destructor Elem_Beam_2D_Nodes2\n\n";
}



void Elem_Beam_2D_Nodes2::prepareElemData()
{
  ElementBase::prepareElemData();
  
  return;
}





int Elem_Beam_2D_Nodes2::calcLoadVector(VectorXd& Flocal)
{
  return 0;
}



int  Elem_Beam_2D_Nodes2::calcStiffnessAndResidual(MatrixXd& Klocal, VectorXd& Flocal)
{
  if( ElemTypeData->getNonlinearGeomFlag() )
    return  calcStiffnessAndResidual_Nonlinear(Klocal, Flocal);
  else
    return  calcStiffnessAndResidual_Linear(Klocal, Flocal);

}




int  Elem_Beam_2D_Nodes2::calcStiffnessAndResidual_Linear(MatrixXd& Klocal, VectorXd& Flocal)
{
    int  ii, jj, gp, kk, ind1, ind2;
    
    double  sth0, cth0, bforce[2];
    double  aa, fact, fact1, fact2, fact3, fact4;
    double  x2, y2, x1, y1, u1, u2, w1, w2, h;

    VectorXd  dispC(6), accC(6), velC(6);
    MatrixXd  Mlocal(6,6), RotMat(6,6), RotMatTrans(6,6);

    double rho0  = MatlData->getDensity() ;
    double rho   = rho0;

    vector<double> matDat = MatlData->getData();
    vector<double> elmDat = ElemTypeData->getData();

    double  E     = matDat[0];
    double  nu    = matDat[1];

    double  A     = elmDat[0];
    double  I     = elmDat[1];
    double  kappa = elmDat[2];
    double  EA    = E*A;
    double  EI    = E*I;


    double       af = SolnData->td(2);
    double acceFact = SolnData->td(5);
    double veloFact = SolnData->td(10);

    //  rotate nodal displacements and compute nodal positions on element axis
    x1 = GeomData->NodePosOrigLocal[nodeInLocalElem[0]][0];
    y1 = GeomData->NodePosOrigLocal[nodeInLocalElem[0]][1];
    x2 = GeomData->NodePosOrigLocal[nodeInLocalElem[1]][0];
    y2 = GeomData->NodePosOrigLocal[nodeInLocalElem[1]][1];

    for(ii=0;ii<npElem;ii++)
    {
      ind1 = ndof*ii;
      ind2 = nodeNums[ii]*ndof;

      for(kk=0;kk<ndof;kk++)
      {
        dispC(ind1+kk)  =  SolnData->dispCur[ind2+kk];
        velC(ind1+kk)   =  SolnData->veloCur[ind2+kk];
        accC(ind1+kk)   =  SolnData->acceCur[ind2+kk];
      }
    }

    // compute the orientation of the element

    h    = sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
    sth0 = (y2-y1)/h;
    cth0 = (x2-x1)/h;

    RotMat.setZero();
    RotMat(0,0) = cth0; RotMat(0,1) = -sth0;
    RotMat(1,0) = sth0; RotMat(1,1) =  cth0;
    RotMat(2,2) = 1.0;

    RotMat(3,3) = cth0; RotMat(3,4) = -sth0;
    RotMat(4,3) = sth0; RotMat(4,4) =  cth0;
    RotMat(5,5) = 1.0;
    
    RotMatTrans = RotMat.transpose();


    Klocal.resize(nsize, nsize);
    Klocal.setZero();
    Flocal.resize(nsize);
    Flocal.setZero();

    fact = EA/h;

    Klocal(0,0) =  fact;
    Klocal(0,3) = -fact;
    Klocal(3,0) = -fact;
    Klocal(3,3) =  fact;

    fact = 12.0*EI/h/h/h;

    Klocal(1,1) =  fact;
    Klocal(1,4) = -fact;
    Klocal(4,1) = -fact;
    Klocal(4,4) =  fact;

    fact = 6.0*EI/h/h;

    Klocal(1,2) =  fact;
    Klocal(2,1) =  fact;
    Klocal(1,5) =  fact;
    Klocal(5,1) =  fact;

    Klocal(2,4) = -fact;
    Klocal(4,2) = -fact;
    Klocal(4,5) = -fact;
    Klocal(5,4) = -fact;

    fact = EI/h;

    Klocal(2,2) =  4.0*fact;
    Klocal(2,5) =  2.0*fact;
    Klocal(5,2) =  2.0*fact;
    Klocal(5,5) =  4.0*fact;

    //printMatrix(Klocal);	printf("\n\n\n");

    //body forces

    //inertia

    Mlocal.setZero();

    Mlocal(0,0) = 140.0;
    Mlocal(0,3) =  70.0;

    Mlocal(1,1) = 156.0;
    Mlocal(1,2) =  22.0*h;
    Mlocal(1,4) =  54.0;
    Mlocal(1,5) = -13.0*h;

    Mlocal(2,1) =  22.0*h;
    Mlocal(2,2) =   4.0*h*h;
    Mlocal(2,4) =  13.0*h;
    Mlocal(2,5) =  -3.0*h*h;

    Mlocal(3,0) = 70.0;
    Mlocal(3,3) = 140.0;

    Mlocal(4,1) = 54.0;
    Mlocal(4,2) = 13.0*h;
    Mlocal(4,4) = 156.0;
    Mlocal(4,5) = -22.0*h;

    Mlocal(5,1) = -13.0*h;
    Mlocal(5,2) = -3.0*h*h;
    Mlocal(5,4) = -22.0*h;
    Mlocal(5,5) = 4.0*h*h;

    fact = rho*A*h/420.0;

    Mlocal = fact*Mlocal;

    //printVector(dispC);
    //printMatrix(Klocal); printf("\n\n");
    //printMatrix(Mlocal); printf("\n\n");

    dispC = RotMatTrans*dispC;
    accC  = RotMatTrans*accC;
    velC  = RotMatTrans*velC;

    Flocal = -Klocal*dispC - Mlocal*accC;
    Klocal = af*Klocal + acceFact*Mlocal;

    Klocal = (RotMat*Klocal)*RotMatTrans;
    Flocal = RotMat*Flocal;

    //printMatrix(Klocal); printf("\n\n");
    //printVector(Flocal); printf("\n\n");

  return 0;
}




int  Elem_Beam_2D_Nodes2::calcStiffnessAndResidual_Nonlinear(MatrixXd& Klocal, VectorXd& Flocal)
{
    int  ii, jj, gp, kk, TI, TIp1, TIp2, TJ, TJp1, TJp2, ind1, ind2;

    double  sth0, cth0, uxn[2], uzn[2], btn[2], x0[2], y0[2], x1[2], y1[2], xx[2];

    double  ux, uz, bt, sbt, cbt, dux, duz, dbt, detJ, h;
    double  K, SF, NF, BM, dvol;
    double  fact, fact1, fact2, fact3, fact4, EAdv, GAdv, EIdv, dx, dy;

    vector<double>  N(npElem), dN_dx(npElem);

    VectorXd  res(3), accC(6), velC(6), dispC(6);
    MatrixXd  Mlocal(6,6), Bi(3,6), Bj(3,3), D(3,3), RotMat(6,6), RotMatTrans(6,6);
    D.setZero();
    Bi.setZero();
    Bj.setZero();

    double rho0  = MatlData->getDensity() ;
    double rho   = rho0;

    double       af = SolnData->td(2);
    double acceFact = SolnData->td(5);

    vector<double> matDat = MatlData->getData();
    vector<double> elmDat = ElemTypeData->getData();

    double  E     = matDat[0];
    double  nu    = matDat[1];

    double  A     = elmDat[0];
    double  I     = elmDat[1];
    double  kappa = elmDat[2];
    double  EA    = E*A;
    double  EI    = E*I;
    double  G     = E/(2.0*(1.0+nu));
    double  GA    = G*A*kappa;
    

    //  rotate nodal displacements and compute nodal positions on element axis

    x0[0] = GeomData->NodePosOrigLocal[nodeInLocalElem[0]][0];
    y0[0] = GeomData->NodePosOrigLocal[nodeInLocalElem[0]][1];
    x0[1] = GeomData->NodePosOrigLocal[nodeInLocalElem[1]][0];
    y0[1] = GeomData->NodePosOrigLocal[nodeInLocalElem[1]][1];

    for(ii=0;ii<npElem;ii++)
    {
      ind1 = ndof*ii;
      ind2 = nodeNums[ii]*ndof;

      for(kk=0;kk<ndof;kk++)
      {
        dispC(ind1+kk)  =  SolnData->dispCur[ind2+kk];
        velC(ind1+kk)   =  SolnData->veloCur[ind2+kk];
        accC(ind1+kk)   =  SolnData->acceCur[ind2+kk];
      }
    }

    x1[0] = x0[0];
    y1[0] = y0[0];
    x1[1] = x0[1];
    y1[1] = y0[1];

    // compute the orientation of the element

    dx = x1[1] - x1[0];
    dy = y1[1] - y1[0];

    h  = sqrt(dx*dx+dy*dy);
    sth0  = dy/h;
    cth0  = dx/h;
    xx[0] = 0.0;
    xx[1] = h;
    
    RotMat.setZero();
    RotMat(0,0) = cth0; RotMat(0,1) = -sth0;
    RotMat(1,0) = sth0; RotMat(1,1) =  cth0;
    RotMat(2,2) = 1.0;

    RotMat(3,3) = cth0; RotMat(3,4) = -sth0;
    RotMat(4,3) = sth0; RotMat(4,4) =  cth0;
    RotMat(5,5) = 1.0;
    
    RotMatTrans = RotMat.transpose();
    
    dispC = RotMatTrans*dispC;
    accC  = RotMatTrans*accC;
    velC  = RotMatTrans*velC;

    uxn[0] = dispC(0);
    uzn[0] = dispC(1);
    btn[0] = dispC(2);
    uxn[1] = dispC(3);
    uzn[1] = dispC(4);
    btn[1] = dispC(5);

    //cout << rho << '\t' << A << '\t' << G << '\t' << EA << '\t' << EI << '\t' << GA << endl;
    //cout << x1[0] << '\t' << y1[0] << '\t' << x1[1] << '\t' << y1[1] << '\t' << h << endl;
    //cout << xx[0] << '\t' << xx[1] << "\n\n" << endl;
    //cout << sth0 << '\t' << cth0 << "\n\n" << endl;
    //cout << " ccccccccccc " << endl;

    nGP = 1;
    vector<double>  gausspoints, gaussweights;
    getGaussPoints1D(nGP, gausspoints, gaussweights);

    if(Klocal.rows() != nsize)
    {
      Klocal.resize(nsize, nsize);
      Flocal.resize(nsize);
    }
    Klocal.setZero();
    Flocal.setZero();


    for(gp=0; gp<nGP; gp++)
    {
        LagrangeBasisFunsLine1D(npElem-1, gausspoints[gp], xx, &N[0], &dN_dx[0], detJ);

        //cout << N[0] << '\t' << N[1] << endl;
        //cout << dN_dx[0] << '\t' << dN_dx[1] << '\t' << detJ << "\n" << endl;

        //compute ux, uz, beta in current Gauss point

        ux = uz = bt = dux = duz = dbt = 0.0;
        for(ii=0;ii<npElem;ii++)
        {
          ux  += uxn[ii] * N[ii];
          uz  += uzn[ii] * N[ii];
          bt  += btn[ii] * N[ii];
          dux += uxn[ii] * dN_dx[ii];
          duz += uzn[ii] * dN_dx[ii];
          dbt += btn[ii] * dN_dx[ii];
        }

        sbt = sin(bt);
        cbt = cos(bt);

        //compute average normal strain, shear strain and curvature

        fact = (1.0+dux)*cbt - duz*sbt;

        E = dux + 0.5*(dux*dux + duz*duz);
        G = (1.0+dux)*sbt + duz*cbt;
        K = dbt * fact;

        //compute material response (elastic)

        NF = EA * E;// normal force
        SF = GA * G;// shear force
        BM = EI * K;// bending moment

        //multiply with volume element

        dvol  = gaussweights[gp] * detJ;
        fact1 = dvol * af;
        NF    = NF * dvol;
        SF    = SF * dvol;
        BM    = BM * dvol;
        EAdv  = EA * fact1;
        GAdv  = GA * fact1;
        EIdv  = EI * fact1;

        fact1 = (+ SF * cbt - BM * dbt * sbt) * af;
        fact2 = (- SF * sbt - BM * dbt * cbt) * af;
        
        D(0,0) = EAdv;
        D(1,1) = GAdv;
        D(2,2) = EIdv;

        for(ii=0;ii<npElem;ii++)
        {
          TI   =  3*ii;
          TIp1 =  TI+1;
          TIp2 =  TI+2;

          Bi(0,TI)   = (1.0+dux) * dN_dx[ii];
          Bi(0,TIp1) = duz * dN_dx[ii];
          Bi(0,TIp2) = 0.0;

          Bi(1,TI)   = sbt * dN_dx[ii];
          Bi(1,TIp1) = cbt * dN_dx[ii];
          Bi(1,TIp2) = fact * N[ii];

          Bi(2,TI)   = dbt*cbt * dN_dx[ii];
          Bi(2,TIp1) = - dbt*sbt * dN_dx[ii];
          Bi(2,TIp2) = fact * dN_dx[ii] - G*dbt * N[ii];
        }

        //printMatrix(Bi);	printf("\n\n\n");
        //printMatrix(D);	printf("\n\n\n");

        res(0) = NF;
        res(1) = SF;
        res(2) = BM;

        Klocal += (Bi.transpose()*(D*Bi));
        Flocal -= (Bi.transpose()*res);

        //printMatrix(Klocal);	printf("\n\n\n");

        //compute geometrical part of stiffness matrix

        for(ii=0;ii<npElem;ii++)
        {
          TI   =  3*ii;
          TIp1 =  TI+1;
          TIp2 =  TI+2;

          for(jj=0;jj<npElem;jj++)
          {
            TJ   = 3*jj;
            TJp1 = TJ+1;
            TJp2 = TJ+2;

            Klocal(TI,TJ)     += ( dN_dx[ii]*NF*dN_dx[jj] * af);
            Klocal(TIp1,TJp1) += ( dN_dx[ii]*NF*dN_dx[jj] * af);

            fact3 =  dN_dx[ii]*BM*cbt*dN_dx[jj] * af;
            fact4 = -dN_dx[ii]*BM*sbt*dN_dx[jj] * af;

            Klocal(TI,TJp2)   += (fact3 + dN_dx[ii]*fact1*N[jj] );
            Klocal(TIp1,TJp2) += (fact4 + dN_dx[ii]*fact2*N[jj] );
            Klocal(TIp2,TJ)   += (fact3 + N[ii]*fact1*dN_dx[jj] );
            Klocal(TIp2,TJp1) += (fact4 + N[ii]*fact2*dN_dx[jj] );

            Klocal(TIp2,TJp2) += (N[ii]*(-SF*G-BM*dbt*fact)*N[jj] - dN_dx[ii]*BM*G*N[jj] - N[ii]*BM*G*dN_dx[jj]) * af;
          }//for(jj
        }//for(ii

    }//for(gp

    //printMatrix(Klocal); printf("\n\n");
    //printVector(Flocal); printf("\n\n");

    //body forces

    //inertia
    fact1 = rho*A*h/6.0;
    fact2 = rho*I*h/6.0;

    fact3 = 2.0*fact1;
    fact4 = 2.0*fact2;
    
    Mlocal.setZero();

    Mlocal(0,0) = fact3;    Mlocal(3,3) = fact3;    Mlocal(0,3) = fact1;    Mlocal(3,0) = fact1;
    Mlocal(1,1) = fact3;    Mlocal(4,4) = fact3;    Mlocal(1,4) = fact1;    Mlocal(4,1) = fact1;
    Mlocal(2,2) = fact4;    Mlocal(5,5) = fact4;    Mlocal(2,5) = fact2;    Mlocal(5,2) = fact2;

    Klocal += (acceFact*Mlocal);
    Flocal -= (Mlocal*accC);

    Klocal = (RotMat*Klocal)*RotMatTrans;
    Flocal = RotMat*Flocal;

    //printMatrix(Klocal); printf("\n\n");
    //printVector(Flocal); printf("\n\n");

    return 0;
}





int Elem_Beam_2D_Nodes2::calcInternalForces()
{
  return 0;
}



void Elem_Beam_2D_Nodes2::elementContourplot(int vartype, int varindex, int index)
{
  return;
}


void Elem_Beam_2D_Nodes2::projectToNodes(bool extrapolateFlag, int vartype, int varindex, int index)
{
  return;
}


void Elem_Beam_2D_Nodes2::projectStress(bool extrapolateFlag, int vartype, int varindex, int index, double* outval)
{
  return;
}



void Elem_Beam_2D_Nodes2::projectStrain(bool extrapolateFlag, int vartype, int varindex, int index, double* outval)
{
  return;
}



void Elem_Beam_2D_Nodes2::projectInternalVariable(bool extrapolateFlag, int vartype, int varindex, int index, double* outval)
{
  return;
}



int Elem_Beam_2D_Nodes2::calcOutput(double u1, double v1)
{
  return 0;
}

